/*********************************************************************\
*
*                  (c) lkc
*                  All Rights Reserved. lkc Confidential
*
\*********************************************************************/
/*********************************************************************\
*   Title                      :
*
*   Module Description         :
*
*   Author                     : lkc
*
*   Created                    : 2018-6-24
*
\*********************************************************************/

#include <chrono>
#include <QDebug>
#include <thread>
#include <QThread>
#include "UtilityTH.h"
#include "ImageDetect.h"
#include "V4l2Device.h"
#include "CamGainControl.h"

ImageDetect ImageDetect::m_Instance;

ImageDetect& ImageDetect::GetInstance()
{
    return m_Instance;
}

ImageDetect::ImageDetect()
{
    m_dstImage = cv::Mat::zeros(HEIGHT, WIDTH, CV_8UC3);
}

//ImageDetect::~ImageDetect()
//{
//}

void ImageDetect::DetectThread()
{
//    std::thread(std::bind(&ImageDetect::Detect,this)).detach();
    std::thread(&ImageDetect::Detect,this).detach();
}

std::vector <cv::Point2d> ImageDetect::GetObjectPoints()
{
    m_objectPointsMtx.lock();
    std::vector <cv::Point2d> retult(m_objectPoints);
    m_objectPointsMtx.unlock();
    return retult;
}

void ImageDetect::Detect()
{
    while(true)
    {
        auto startTime = std::chrono::system_clock::now();
        cv::Mat matFrame;
        v4l2_buffer buf;
        auto ret = v4lDev.GetFrame(buf, matFrame);
        if(!ret)
        {
            qDebug() <<"GetFrame fail";
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
            continue;
        }

        auto timeLast = m_frameInf.timestamp;
        m_frameInf.timestamp = buf.timestamp;
        long long interval_msec = (long long)(m_frameInf.timestamp.tv_sec-timeLast.tv_sec)*1000+(m_frameInf.timestamp.tv_usec-timeLast.tv_usec)/1000;
//        printf("timeLast    tv_sec:%d,  tv_usec:%d\n", timeLast.tv_sec, timeLast.tv_usec);
//        printf("timeNow    tv_sec:%d,  tv_usec:%d\n", timeNow.tv_sec, timeNow.tv_usec);
//        printf("interval_msec is %lldms\n",interval_msec);
        if(interval_msec<=0)
        {
//            printf("buf data has not updated, wait for next time\n");
            v4lDev.ReleaseBuf(buf);
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            continue;
        }
//        printf("buf data has updated, start image detect\n");
        cv::Mat tmp;
        matFrame.copyTo(tmp);
        v4lDev.ReleaseBuf(buf);
        FrameDetect(tmp);
        auto endTime = std::chrono::system_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(endTime - startTime);
//        qDebug() <<"ImageDetect::Detect cost" << static_cast<double>(duration.count())<< " us";
    }
}

//单帧中白点数目大于一定值(不同增益下，该值不一样)，即认为单帧存在目标，是否真的存在目标需要通过多帧计算，cv::contourArea貌似有问题
void ImageDetect::FrameDetect(cv::Mat &img)
{
    auto startTimeFrameProcess = std::chrono::system_clock::now();
//    cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2, 2));
//    cv::morphologyEx(img, img, cv::MORPH_OPEN, element);
//    cv::medianBlur(img, img,3);
    cv::Mat thresholdImg;
    double threValue = cv::threshold(img, thresholdImg, 10, 255, cv::THRESH_OTSU);         //OSTU方式寻找最佳的阈值

    auto endTime = std::chrono::system_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(endTime - startTimeFrameProcess);
//    qDebug() <<"cv::threshold cost" << static_cast<double>(duration.count())<< " us";

    if(threValue<22)
    {
        threValue = 22;
        cv::threshold(img, thresholdImg, threValue, 255, cv::THRESH_BINARY);
    }
    if(threValue>100)
    {
        threValue = 100;
        cv::threshold(img, thresholdImg, threValue, 255, cv::THRESH_BINARY);
    }
//    printf("thresholdValue is %f\n",threValue);

//    cv::Mat thresholdImgTemp = thresholdImg.clone();

//    cv::Scalar meanVal=cv::mean(thresholdImg);
//    m_whitePixNum = static_cast<int>(meanVal.val[0]*WIDTH*HEIGHT/255);//meanVal.val[0]表示第一个通道的均值
    m_whitePixNum = cv::countNonZero(thresholdImg);
//    printf("白点数目为%d\n",m_whitePixNum);

    std::vector < std::vector<cv::Point> > contours;
    cv::findContours(thresholdImg, contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);
    //轮廓排序，之后的处理中，只考虑前面的大轮廓
    std::sort(contours.begin(), contours.end(),
        [](const std::vector<cv::Point> &contour1, const std::vector<cv::Point> &contour2)
    {
        return (cv::contourArea(contour1) > cv::contourArea(contour2));
    });

    DetectBigBlobs(contours);//找到大的斑点，删去太靠近的斑点
    TargetSameLocationConfirm();
    CheckTargetLost();
    if(CamGainControl::GetInstance().GetCamGainMode())
    {
       CamGainAutoSet();
    }
//    UpdateFrameInfVec();
    UpdateObjectPoints();
    ShowImage(img);

//    cv::Mat drawContoursImg = cv::Mat::zeros(HEIGHT, WIDTH, CV_8UC3);
//    cv::drawContours(drawContoursImg, contours, -1, cv::Scalar(255, 0, 0), 2, 8);//颜色标记
//    for (size_t i = 0; i < m_frameInf.blobs.size(); i++)
//    {
//        std::string str1 = std::to_string(i);
//        cv::circle( drawContoursImg, m_frameInf.blobs[i].location, m_frameInf.blobs[i].radius, CV_RGB(255, 0, 0), 1, CV_AA);
//        cv::putText(drawContoursImg, str1, cv::Point(m_frameInf.blobs[i].location.x-15, m_frameInf.blobs[i].location.y+15), cv::FONT_HERSHEY_SIMPLEX | cv::FONT_ITALIC, 0.5, CV_RGB(255, 0, 0), 1);//在图片中输出字符
//    }
//    cv::Mat drawContoursImgResize;
//    cv::resize(drawContoursImg, drawContoursImgResize, cv::Size(WIDTH/ 2, HEIGHT/ 2));
//    cv::Mat imgResize;
//    cvtColor(img, img, CV_GRAY2BGR );
//    cv::resize(img, imgResize, cv::Size(WIDTH/ 2, HEIGHT/ 2));
//    cv::Mat thresholdImgTempResize;
//    cvtColor(thresholdImgTemp, thresholdImgTemp, CV_GRAY2BGR );
//    cv::resize(thresholdImgTemp, thresholdImgTempResize, cv::Size(WIDTH/ 2, HEIGHT/ 2));
//    cv::Mat roi1 = m_dstImage(cv::Rect(0,0,WIDTH/2, HEIGHT/2));
//    cv::Mat roi2 = m_dstImage(cv::Rect(WIDTH/2,0,WIDTH/2, HEIGHT/2));
//    cv::Mat roi3 = m_dstImage(cv::Rect(0,HEIGHT/2,WIDTH/2, HEIGHT/2));

//    m_dstImageMtx.lock();
//    imgResize.copyTo(roi1);
//    thresholdImgTempResize.copyTo(roi2);
//    drawContoursImgResize.copyTo(roi3);
//    m_dstImageMtx.unlock();
//    ShowImage(img);

    endTime = std::chrono::system_clock::now();
    duration = std::chrono::duration_cast<std::chrono::microseconds>(endTime - startTimeFrameProcess);
//    qDebug() <<"FrameDetect cost" << static_cast<double>(duration.count())<< " us";
}

//认为一帧中位置接近的点是同一个点,找到大的斑点，删去太靠近的斑点           接下去可以考虑聚类
void ImageDetect::DetectBigBlobs(std::vector<std::vector<cv::Point>> &sortedContours)
{
    m_frameInf.blobs.clear();
    unsigned int number = (sortedContours.size()>BLOB_NUM_MAX)?BLOB_NUM_MAX:sortedContours.size();
    for(unsigned int i = 0; i < number; i++ )
    {
        double area = cv::contourArea(sortedContours[i]);
        unsigned int contoursSize = sortedContours[i].size();
//        printf("contours_%d, size is %d,area is %f\n",i,contoursSize,area);
        if(area<BLOB_AREA_MIN)
        {
            continue;
        }
        Blob blob;
        blob.area = area;
        float pointx =0,pointy=0;
        for(unsigned int j=0; j<contoursSize;j++)
        {
            pointx+=sortedContours[i][j].x;
            pointy+=sortedContours[i][j].y;
        }
        pointx = pointx/contoursSize;
        pointy = pointy/contoursSize;

        blob.location = cv::Point2d(pointx, pointy);

        double distSum = 0;
        for(unsigned int j=0;j<contoursSize;j++)
        {
            distSum += sqrt(pow(blob.location.x-sortedContours[i][j].x,2)+pow(blob.location.y-sortedContours[i][j].y,2));
        }
        blob.radius=distSum/contoursSize;
//        printf("blob_%d,location_x is %f,location_y is %f\n",i,blob.location.x,blob.location.y);
//        printf("blob_%d, area is %f\n",i,blob.area);
//        printf("blob_%d, radius is %f\n",i,blob.radius);
        m_frameInf.blobs.push_back(std::move(blob));
    }

    //去掉比较靠近的blob对当中比较小的那个blob
    for(auto it1 = m_frameInf.blobs.begin();it1!=m_frameInf.blobs.end();it1++)
    {
        for(auto it2 = it1+1;it2!=m_frameInf.blobs.end();)
        {
            double dist = sqrt(pow(it1->location.x-it2->location.x,2)+pow(it1->location.y-it2->location.y,2));
            if(dist<it1->radius+it2->radius+10)
            {
                m_frameInf.blobs.erase(it2);
            }
            else
            {
                it2++;
            }
        }
    }
}

////还需要考虑前后连续几帧 同一个位置有点  才认为有目标
//void ImageDetect::TargetSameLocationConfirm()
//{
////    UpdateFrameInfVec();
//    for(size_t i=0;i<m_frameInf.blobs.size();i++)
//    {
//        if(m_frameInfVec.size()<FRAME_NUM+1)
//        {
//            break;
//        }
//        Blob blobA = m_frameInf.blobs[i];
//        for(int j=2;j<=FRAME_NUM+1;j++)
//        {
//            int index = m_frameInfVec.size()-j;
//            for(size_t k=0;k<m_frameInfVec[index].blobs.size();k++)
//            {
//                Blob blobB = m_frameInfVec[index].blobs[k];
//                double dist = sqrt(pow(blobA.location.x-blobB.location.x,2)+pow(blobA.location.y-blobB.location.y,2));
//                if(dist < blobA.radius+blobB.radius+50)
//                {
//                   m_frameInf.blobs[i].sameLocationCount++;
//                   break;
//                }
//            }
//        }
//    }
////    UpdateObjectPoints();
//}

//还需要考虑前后连续几帧 同一个位置有点  才认为有目标  如果闪烁频率为0.1 1秒25帧 只有2.5帧有信号 无法进行前后帧判断
void ImageDetect::TargetSameLocationConfirm()
{
    for(size_t i=0;i<m_frameInf.blobs.size();i++)
    {
        if(m_frameInfVec.size()<SAME_POSITION_FRAME_NUM)
        {
            break;
        }
        Blob blobA = m_frameInf.blobs[i];
        for(int j=1;j<=SAME_POSITION_FRAME_NUM;j++)
        {
            int index = m_frameInfVec.size()-j;
            for(size_t k=0;k<m_frameInfVec[index].blobs.size();k++)
            {
                Blob blobB = m_frameInfVec[index].blobs[k];
                double dist = sqrt(pow(blobA.location.x-blobB.location.x,2)+pow(blobA.location.y-blobB.location.y,2));
                if(dist < blobA.radius+blobB.radius+SAME_POSITION_DIST*j)//前面第一帧允许最大距离为SAME_POSITION_DIST，前面第二帧允许最大距离为SAME_POSITION_DIST*2，以此类推
                {
                   m_frameInf.blobs[i].sameLocationCount++;
                   break;
                }
            }
        }
    }
}

////根据前后连续几帧判断结果，确认当前帧有几个目标
//void ImageDetect::UpdateObjectPoints()
//{
//    m_objectPointsMtx.lock();
//    m_objectPoints.clear();
//    unsigned int number = (m_frameInf.blobs.size()>TARGET_NUM_MAX)?TARGET_NUM_MAX:m_frameInf.blobs.size();
//    for(size_t i=0;i<number;i++)
//    {
//        if(m_frameInf.blobs[i].sameLocationCount>=SAME_POSITION_FRAME_NUM)
//        {
//            m_objectPoints.push_back(m_frameInf.blobs[i].location);
//        }
//    }
//    m_objectPointsMtx.unlock();
//}

//根据前后连续几帧判断结果，确认当前帧有几个目标，但是因为闪烁的原因当前帧没有目标，保持之前的目标信息
void ImageDetect::UpdateObjectPoints()
{
    m_objectPointsMtx.lock();
    if(m_targetIsLost)
    {
         m_objectPoints.clear();
         m_objectPointsMtx.unlock();
         return;
    }
    std::vector <cv::Point2d> objectPointsTemp;
    unsigned int number = (m_frameInf.blobs.size()>TARGET_NUM_MAX)?TARGET_NUM_MAX:m_frameInf.blobs.size();
    for(size_t i=0;i<number;i++)
    {
        if((m_frameInf.blobs[i].sameLocationCount>=SAME_POSITION_FRAME_NUM)||(m_frameInf.blobs[i].area>BLOB_AREA_CONFIRM)||(m_whitePixNum>WHITE_PIX_NUM_CONFIRM))//前后帧判断在相近的位置，或者当前帧目标点很大，则认为该点是目标点
        {
            objectPointsTemp.push_back(m_frameInf.blobs[i].location);
        }
    }
    if(objectPointsTemp.size()>0)
    {
        m_objectPoints.clear();
        m_objectPoints.insert(m_objectPoints.end(),objectPointsTemp.begin(),objectPointsTemp.end());
    }
    m_objectPointsMtx.unlock();
}

////不根据前后连续几帧判断结果，直接确认当前帧有几个目标
//void ImageDetect::UpdateObjectPoints()
//{
//    m_objectPointsMtx.lock();
//    m_objectPoints.clear();
//    unsigned int number = (m_frameInf.blobs.size()>TARGET_NUM_MAX)?TARGET_NUM_MAX:m_frameInf.blobs.size();
//    for(size_t i=0;i<number;i++)
//    {
//        m_objectPoints.push_back(m_frameInf.blobs[i].location);
//    }
//    m_objectPointsMtx.unlock();
//}

////不根据前后连续几帧判断结果，直接确认当前帧有几个目标,若目标没有彻底丢失，但是因为闪烁的原因当前帧没有目标，保持之前的目标信息
//void ImageDetect::UpdateObjectPoints()
//{
//    m_objectPointsMtx.lock();
//    if(m_targetIsLost)
//    {
//         m_objectPoints.clear();
//         m_objectPointsMtx.unlock();
//         return;
//    }
//    unsigned int number = (m_frameInf.blobs.size()>TARGET_NUM_MAX)?TARGET_NUM_MAX:m_frameInf.blobs.size();
//    if(number>0)
//    {
//        m_objectPoints.clear();
//        for(size_t i=0;i<number;i++)
//        {
//            m_objectPoints.push_back(m_frameInf.blobs[i].location);
//        }
//    }

//    m_objectPointsMtx.unlock();
//}


void ImageDetect::UpdateFrameInfVec()
{
    //m_frameInfVec中存储到目前帧为止的FRAME_COUNT_SIZE帧
    m_frameInfVec.push_back(m_frameInf);
//    printf("m_frameInfVec size is %d\n",m_frameInfVec.size());
    while(m_frameInfVec.size()>FRAME_COUNT_SIZE)
    {
        m_frameInfVec.erase(m_frameInfVec.begin());
//        printf("after erase, m_frameInfVec size is %d\n",m_frameInfVec.size());
    }
//    printf("now TargetConfirm by last N frame\n");
//    long long interval_msec = (long long)(m_frameInf.timestamp.tv_sec-m_frameInfVec[0].timestamp.tv_sec)*1000+(m_frameInf.timestamp.tv_usec-m_frameInfVec[0].timestamp.tv_usec)/1000;
//    printf("last %d frame,time interval is %lldms\n",m_frameInfVec.size(),interval_msec);
}


void ImageDetect::CheckTargetLost()
{
    m_frameInf.hasSuspectedTarget = false;
    if(m_whitePixNum>2000)
    {
        m_frameInf.hasSuspectedTarget = true;
    }

    for(size_t i = 0;i<m_frameInf.blobs.size();i++)
    {
        double blobArea = m_frameInf.blobs[i].area;
        if(blobArea>=BLOB_AREA_MIN)
        {
            m_frameInf.hasSuspectedTarget = true;
            break;
        }
    }

    UpdateFrameInfVec();

    int targetCount = 0;
    for(size_t i=0;i<m_frameInfVec.size();i++)
    {
        if(m_frameInfVec[i].hasSuspectedTarget)
        {
            targetCount ++;
        }
    }
    float ratio = targetCount*1.0/m_frameInfVec.size();
//    printf("target ratio is %f\n",ratio);

    if(!(m_frameInf.hasSuspectedTarget)&&(ratio<0.05))//前后帧判断会减小有效帧的使用，因此只根据每一帧是否有疑似目标判断目标是否丢失,而且有大噪点的情况，很有可能是有目标存在,随机噪声不太可能在1秒内频繁出现
    {
        m_targetIsLost = true;
    }
    else
    {
        m_targetIsLost = false;
    }
}

static void IncGainMax(int gainInc,int maxGain)//增大增益，但是有最大增益限制,增益增大值为gainInc，最大增益为maxGain
{
    if((CamGainControl::GetInstance().GetGain()+gainInc)>maxGain)
    {
        CamGainControl::GetInstance().SetGain(maxGain);
    }
    else
    {
        CamGainControl::GetInstance().IncGain(gainInc);
    }
}

//static void DecGainMin(int gainDec,int minGain)//减小增益，但是有最小增益限制,增益减小值为gainDec，最小增益为minGain
//{
//    if((CamGainControl::GetInstance().GetGain()-gainDec)<minGain)
//    {
//        CamGainControl::GetInstance().SetGain(minGain);
//    }
//    else
//    {
//        CamGainControl::GetInstance().DecGain(gainDec);
//    }
//}

//void ImageDetect::CamGainAutoSet()//根据图中白点数目以及检测到的白色斑点数量、大小调整增益
//{
//    //确定目标丢失后开始增加增益
//    if(m_targetIsLost)
//    {
//        CamGainControl::GetInstance().IncGain(20);
//        return;
//    }

//    int minGainFar = 50;//信标还比较远时，或者说图像中目标点比较小时，设定一个最小增益

////    printf("before gainset gain is %d\n",CamGainControl::GetInstance().GetGain());
////    printf("m_whitePixNum is %d, #####################################\n",m_whitePixNum);

//    if(m_whitePixNum>2000)
//    {
//        if(m_whitePixNum>10000)
//        {
//            DecGainMin(50,minGainFar);
////           printf("after gainset gain is %d\n",CamGainControl::GetInstance().GetGain());
////           printf("m_whitePixNum is %d,>(10000) #####################################\n",m_whitePixNum);
//           return;
//        }
//        else if(m_whitePixNum>3000)
//        {
//            DecGainMin(30,minGainFar);
////           printf("after gainset gain is %d\n",CamGainControl::GetInstance().GetGain());
////           printf("m_whitePixNum>(3000) #####################################\n");
//           return;
//        }
//        else
//        {
//            DecGainMin(20,minGainFar);
////           printf("after gainset gain is %d\n",CamGainControl::GetInstance().GetGain());
////           printf("m_whitePixNum>(2000) #####################################\n");
//           return;
//        }
//    }

//    for(size_t i = 0;i<m_frameInf.blobs.size();i++)
//    {
//        double blobArea = m_frameInf.blobs[i].area;

//        if(blobArea>1000)
//        {
//            DecGainMin(10,minGainFar);
////            printf("after gainset gain is %d\n",CamGainControl::GetInstance().GetGain());
////            printf("blobArea>1000 #####################################\n");
//        }
//        else if(blobArea>200)
//        {
//            DecGainMin(5,minGainFar);
////            printf("after gainset gain is %d\n",CamGainControl::GetInstance().GetGain());
////            printf("blobArea>200 #####################################\n");
//        }
//    }
//}

void ImageDetect::CamGainAutoSet()//根据图中白点数目以及检测到的白色斑点数量、大小调整增益
{
    //确定目标丢失后开始增加增益
    if(m_targetIsLost)
    {
//        CamGainControl::GetInstance().IncGain(20);
        IncGainMax(20,65);
        return;
    }

//    printf("m_whitePixNum is %d, #####################################\n",m_whitePixNum);
//    for(size_t i = 0;i<m_frameInf.blobs.size();i++)
//    {
//        printf("blob_%d area is %.1f\n",i,m_frameInf.blobs[i].area);
//    }
//    printf("before gainset gain is %d\n",CamGainControl::GetInstance().GetGain());

    if(m_whitePixNum<=2000)
    {
        return;
    }

    if(m_whitePixNum>80000)
    {
//            DecGainMin(30,35);
        CamGainControl::GetInstance().DecGain(15);
//        printf("m_whitePixNum is %d, #####################################\n",m_whitePixNum);
    }
    else if(m_whitePixNum>10000)
    {
        CamGainControl::GetInstance().DecGain(8);
//           printf("after gainset gain is %d\n",CamGainControl::GetInstance().GetGain());
//           printf("m_whitePixNum is %d,>(10000) #####################################\n",m_whitePixNum);

    }
    else if(m_whitePixNum>3000)
    {
        CamGainControl::GetInstance().DecGain(5);
//           printf("after gainset gain is %d\n",CamGainControl::GetInstance().GetGain());
//           printf("m_whitePixNum>(3000) #####################################\n");

    }
    else
    {
        CamGainControl::GetInstance().DecGain(3);
//           printf("after gainset gain is %d\n",CamGainControl::GetInstance().GetGain());
//           printf("m_whitePixNum>(2000) #####################################\n");

    }

}

void ImageDetect::ShowImage(const cv::Mat &image)
{
    m_dstImageMtx.lock();
//    m_dstImage = image;
    if( image.type() == CV_8UC3 )
    {
        image.copyTo( m_dstImage );
    }
    else if( image.type() == CV_8UC1 )
    {
        cvtColor(image, m_dstImage, cv::COLOR_GRAY2BGR);
    }
    unsigned int number = (m_frameInf.blobs.size()>TARGET_NUM_MAX)?TARGET_NUM_MAX:m_frameInf.blobs.size();
    for (size_t i = 0; i < number; i++)
    {
        if(m_frameInf.blobs[i].sameLocationCount>=SAME_POSITION_FRAME_NUM)
        {
            std::string str1 = std::to_string(i);
            cv::circle( m_dstImage, m_frameInf.blobs[i].location, m_frameInf.blobs[i].radius, CV_RGB(255, 0, 0), 1, cv::LINE_AA);
            cv::putText(m_dstImage, str1, cv::Point(m_frameInf.blobs[i].location.x-15, m_frameInf.blobs[i].location.y+15), cv::FONT_HERSHEY_SIMPLEX | cv::FONT_ITALIC, 0.5, CV_RGB(255, 0, 0), 1);//在图片中输出字符
        }
    }
    m_dstImageMtx.unlock();
}


/********************************************************************\
 *
 * REVISION RECORDS
 *
\*********************************************************************/
/*********************************************************************/
/*
 *
 *
 *
\*********************************************************************/
/*------------------------------The End------------------------------*/
